/**
 * @file:          FalLinear.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.11.13
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.11.13,14:28:34
 */

/* Include Files **************************************************************/
#include "FalLinear.h"
#include "malloc.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/

void FalLinear_Init(struct FalLinear *self)
{
    self->Run = FalLinear_Run;

    /* init function */
    if (self->xn != NULL)
    {
        free(self->xn);
    }
    self->xn = (float *)malloc(sizeof(float) * (self->param_section_nums + 1));

    if (self->yn != NULL)
    {
        free(self->yn);
    }
    self->yn = (float *)malloc(sizeof(float) * (self->param_section_nums + 1));

    if (self->k != NULL)
    {
        free(self->k);
    }
    self->k = (float *)malloc(sizeof(float) * (self->param_section_nums));

    if (self->b != NULL)
    {
        free(self->b);
    }
    self->b = (float *)malloc(sizeof(float) * (self->param_section_nums));

    self->xn[0] = 0;
    self->yn[0] = 0;
    self->xn[1] = self->param_d;
    self->yn[1] = fal(self->xn[1], self->param_a, self->param_d);
    self->k[0] = 1.0F / (powf(self->param_d, 1 - self->param_a));
    self->b[0] = 0;

    for (size_t i = 2; i < self->param_section_nums + 1; i++)
    {
        self->xn[i] = self->param_inteval_gain * self->xn[i - 1];
        self->yn[i] = fal(self->xn[i], self->param_a, self->param_d);
    }

    for (size_t i = 1; i < self->param_section_nums; i++)
    {
        self->k[i] = (self->yn[i + 1] - self->yn[i]) / (self->xn[i + 1] - self->xn[i]);
        self->b[i] = self->yn[i] - self->k[i] * self->xn[i];
    }
}

float FalLinear_Run(struct FalLinear *self, float x)
{
    if (self->param_en != 0)
    {
        float x_abs = fabsf(x / self->param_zoom);
        uint16_t i;
        for (i = 0; i < self->param_section_nums - 1; i++)
        {
            if (x_abs <= self->xn[i + 1])
            {
                self->out = (self->k[i] * x_abs + self->b[i]) * sign_bm(x) * self->param_zoom;
                return self->out;
            }
        }
        self->out = (self->k[i] * x_abs + self->b[i]) * sign_bm(x) * self->param_zoom;
        return self->out;
        // if (x_abs <= self->param_d)
        // {
        //     self->out = self->k[0] * x;
        // }
        // else
        // {
        //     // int32_t x_sign = sign_bm(x);

        //     // // self->out = powf(fabsf(x), self->param_a) * x_sign; //4.38us
        //     // self->out = powf(x_sign * x, self->param_a) * x_sign; // 4.38us

        //     self->out =
        // }
    }
    else
    {
        self->out = x;
    }

    return self->out;
}
